#ifndef _GLUEPOSE_H
#define _GLUEPOSE_H
#include <math.h>
// Rajout � la simulation et au drone de structures type mrpt
class GPose3D
{
  protected :
   float  m_yaw, m_pitch, m_roll;
  public :
    /** Constructor with initilization of the pose; (remember that angles are always given in radians!)  */
    float m_x,m_y,m_z;
    GPose3D();
    GPose3D(const float x,const float  y,const float  z,const float  yaw=0, const float  pitch=0, const float roll=0);
    inline float x() { return m_x;}
    inline float y() { return m_y;}
    inline float z() { return m_z;}
    inline void x(float x_) { m_x=x_;};
    inline void y(float y_) { m_y=y_;};
    inline void z(float z_) { m_z=z_;};
    inline float yaw() { return m_yaw;}
    inline float pitch() { return m_pitch;}
    inline float roll() { return m_roll;}
    inline void x_incr(const float Ax) {m_x+=Ax;}
    inline void y_incr(const float Ay) {m_y+=Ay;}
    inline void z_incr(const float Az) {m_z+=Az;}
    inline void setYawPitchRoll(float Yaw,float Pitch, float Roll)
      {
        m_yaw=Yaw;
        m_pitch=Pitch;
        m_roll=Roll;
      }
    inline void setYawPitchRoll(double Yaw, double Pitch, double Roll)
      {
        m_yaw=(float) Yaw;
        m_pitch=(float) Pitch;
        m_roll=(float) Roll;
      }  
};


class GPoint2D
{
  public :
    float x,y;
    inline GPoint2D(){x=y=0;}
    inline GPoint2D(double x_, double y_)
    {
      x=(float) x_;
      y=(float) y_;  
    }
    inline GPoint2D(float x_, float y_)
    {
      x=x_;
      y=y_;
    }
    inline GPoint2D &operator-=(const GPoint2D &p)
    {
      x-=p.x;
      y-=p.y;
      return *this;
    }
    inline GPoint2D &operator- (const GPoint2D &p) const
    {
      GPoint2D r(*this);
      return r-=p;
    }
};
class GPoint3D
{
  public :
    float x,y,z;
    inline GPoint3D()
    {
      x=y=z=0;
    }
    inline GPoint3D(double x_, double y_, double z_)
    {
      x=(float) x_;
      y=(float) y_;
      z=(float) z_;  
    }
    inline GPoint3D(float x_, float y_, float z_)
    {
      x=x_;
      y=y_;
      z=z_;
    }
    inline GPoint3D &operator-=(const GPoint3D &p)
    {
      x-=p.x;
      y-=p.y;
      z-=p.z;
      return *this;
    }
    inline GPoint3D &operator- (const GPoint3D &p) const
    {
      GPoint3D r(*this);
      return r-=p;
    }
    inline double norm() const
    {
      return sqrt(x*x+y*y+z*z);
    }
    inline double normxy() const
    {
    	return sqrt(x*x+y*y);
    }
};
#endif
